

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "platform.h"


#include "common/axis.h"
#include "common/maths.h"

#include "config/config_eeprom.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"

#include "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/dma.h"
#include "drivers/io.h"
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/compass/compass.h"
#include "drivers/rx_pwm.h"
#include "drivers/pwm_output.h"
#include "drivers/adc.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/exti.h"
#include "drivers/max7219.h"

#include "fc/config.h"
#include "fc/fc_init.h"
#include "fc/fc_msp.h"
#include "fc/fc_tasks.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"

#include "msp/msp_serial.h"

#include "rx/rx.h"

#include "io/beeper.h"
#include "io/serial.h"
#include "io/gimbal.h"

#include "scheduler/scheduler.h"

#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/initialisation.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"

#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"

#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif

#include "build/build_config.h"
#include "build/debug.h"

#ifdef TARGET_PREINIT
void targetPreInit(void);
#endif

#ifdef SOFTSERIAL_LOOPBACK
serialPort_t *loopbackPort;
#endif

uint8_t systemState = SYSTEM_STATE_INITIALISING;



#ifdef PERFORM_TEST
    static uint16_t pfmPin;
    GPIO_TypeDef* pfmGpio;
    #define PFM_H pfmGpio->BSRR = pfmPin
    #define PFM_L pfmGpio->BRR = pfmPin
    volatile int data0[64];
    volatile int data1[64];
    volatile int data2[64];
void performTest(){
    IO_t pfm = IOGetByTag(IO_TAG(PERFORM_LED_PIN));
    pfmPin = IO_Pin(pfm);
    pfmGpio = IO_GPIO(pfm);
    IOConfigGPIO(pfm, IO_CONFIG(GPIO_Mode_Out_PP,      GPIO_Speed_2MHz));

    while(1){
        PFM_H;
        // {
        //     for(int j=0;j<36750;j++){//FAST ~ 1s
        //         for(int i=0;i<64;i++){
        //             data2[i] = data0[i] * data1[i];
        //         }
        //     }
        // }
        delay(500);
        PFM_L;
        // {
        //     for(int j=0;j<36750;j++){
        //         for(int i=0;i<64;i++){
        //             data2[i] = data0[i] * data1[i];
        //         }
        //     }
        // }
        delay(500);
    }
}
#endif

#ifdef PIN_TEST

void pinH(int pin){
    IO_t pfm = IOGetByTag(pin);
    uint16_t pfmPin = IO_Pin(pfm);
    GPIO_TypeDef* pfmGpio = IO_GPIO(pfm);
    IOConfigGPIO(pfm, IO_CONFIG(GPIO_Mode_Out_PP,      GPIO_Speed_2MHz));

    pfmGpio->BSRR = pfmPin;

}
void pinL(int pin){
    IO_t pfm = IOGetByTag(pin);
    uint16_t pfmPin = IO_Pin(pfm);
    GPIO_TypeDef* pfmGpio = IO_GPIO(pfm);
    IOConfigGPIO(pfm, IO_CONFIG(GPIO_Mode_Out_PP,      GPIO_Speed_2MHz));

    pfmGpio->BRR = pfmPin;
    
}
void pinTest(){
    int i;
    int arr[] = {
                              
//  IO_TAG(PB11),       
//  IO_TAG(PB10),       
//  IO_TAG(PB1 ),       
//  IO_TAG(PB0 ),       
//  IO_TAG(PA7 ),       
//  IO_TAG(PA6 ),       
//  IO_TAG(PA5 ),       
//  IO_TAG(PA4 ),       
//  IO_TAG(PA3 ),       
//  IO_TAG(PA2 ),       
//  IO_TAG(PA1 ),       
//  IO_TAG(PA0 ),       
//  IO_TAG(PC15),       
//  IO_TAG(PC14),       
//  IO_TAG(PC13),//15

 IO_TAG(PB12),
 IO_TAG(PB13),
 IO_TAG(PB14),
 IO_TAG(PB15),
 IO_TAG(PA8 ),
 IO_TAG(PA9 ),
 IO_TAG(PA10),
 IO_TAG(PA11),
 IO_TAG(PA12),
 IO_TAG(PA15),
 IO_TAG(PB3 ),
 IO_TAG(PB4 ),
 IO_TAG(PB5 ),
 IO_TAG(PB6 ),
 IO_TAG(PB7 ),
 IO_TAG(PB8 ),
 IO_TAG(PB9 ),//17


        };
    while(1){
        for(i=0;i<17;i++){
            pinH(arr[i]);
            delay(500);
            pinL(arr[i]);
        }


    }
}
#endif

void init()
{
#ifdef USE_HAL_DRIVER
    HAL_Init();
#endif

    //printfSupportInit();

    systemInit();

    // initialize IO (needed for all IO operations)
    IOInitGlobal();

#ifdef USE_HARDWARE_REVISION_DETECTION
    detectHardwareRevision();
#endif

#ifdef BRUSHED_ESC_AUTODETECT
    detectBrushedESC();
#endif


#ifdef PERFORM_TEST
    performTest();
#endif
#ifdef PIN_TEST
    pinTest();
    while(1);
#endif

    initEEPROM();

    ensureEEPROMContainsValidData();
    readEEPROM();

    /* TODO: Check to be removed when moving to generic targets */
    if (strncasecmp(systemConfig()->boardIdentifier, TARGET_BOARD_IDENTIFIER, sizeof(TARGET_BOARD_IDENTIFIER))) {
       resetEEPROM();
    }

    systemState |= SYSTEM_STATE_CONFIG_LOADED;

    //i2cSetOverclock(masterConfig.i2c_overclock);

    debugMode = systemConfig()->debug_mode;

    // Latch active features to be used for feature() in the remainder of init().
    latchActiveFeatures();

#ifdef TARGET_PREINIT
    targetPreInit();
#endif

    debugInit();
    ledInit();
    LED2_ON;

#ifdef USE_EXTI
    EXTIInit();
#endif

#if defined(BUTTONS)

    buttonsInit();

    // Check status of bind plug and exit if not active
    delayMicroseconds(10);  // allow configuration to settle

    if (!isMPUSoftReset()) {
#if defined(BUTTON_A_PIN) && defined(BUTTON_B_PIN)
        // two buttons required
        uint8_t secondsRemaining = 5;
        bool bothButtonsHeld;
        do {
            bothButtonsHeld = buttonAPressed() && buttonBPressed();
            if (bothButtonsHeld) {
                if (--secondsRemaining == 0) {
                    resetEEPROM();
                    systemReset();
                }
                delay(1000);
                LED0_TOGGLE;
            }
        } while (bothButtonsHeld);
#endif
    }
#endif

#if defined(SPEKTRUM_BIND)
    if (feature(FEATURE_RX_SERIAL)) {
        switch (rxConfig()->serialrx_provider) {
        case SERIALRX_SPEKTRUM1024:
        case SERIALRX_SPEKTRUM2048:
        case SERIALRX_SRXL:
            // Spektrum satellite binding if enabled on startup.
            // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
            // The rest of Spektrum initialization will happen later - via spektrumInit()
            spektrumBind(rxConfigMutable());
            break;
        }
    }
#endif

#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
    // If F4 Overclocking is set and System core clock is not correct a reset is forced
    if (systemConfig()->cpu_overclock && SystemCoreClock != OC_FREQUENCY_HZ) {
        *((uint32_t *)0x2001FFF8) = 0xBABEFACE; // 128KB SRAM STM32F4XX
        __disable_irq();
        NVIC_SystemReset();
    } else if (!systemConfig()->cpu_overclock && SystemCoreClock == OC_FREQUENCY_HZ) {
        *((uint32_t *)0x2001FFF8) = 0x0;        // 128KB SRAM STM32F4XX
        __disable_irq();
        NVIC_SystemReset();
    }

#endif

    delay(100);

    timerInit();  // timer must be initialized before any channel is allocated

#ifdef BUS_SWITCH_PIN
    busSwitchInit();
#endif

#if defined(USE_UART)
    uartPinConfigure(serialPinConfig());
#endif

#if defined(AVOID_UART1_FOR_PWM_PPM)
    serialInit(feature(FEATURE_SOFTSERIAL),
            feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART2_FOR_PWM_PPM)
    serialInit(feature(FEATURE_SOFTSERIAL),
            feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE);
#elif defined(AVOID_UART3_FOR_PWM_PPM)
    serialInit(feature(FEATURE_SOFTSERIAL),
            feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE);
#else
    serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
#endif

    mixerInit();
    mixerConfigureOutput();

    uint16_t idlePulse = motorConfig()->mincommand;
    if (feature(FEATURE_3D)) {
        idlePulse = flight3DConfig()->neutral3d;
    }
    if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
        featureClear(FEATURE_3D);
        idlePulse = 0; // brushed motors
    }
    /* Motors needs to be initialized soon as posible because hardware initialization 
     * may send spurious pulses to esc's causing their early initialization. Also ppm
     * receiver may share timer with motors so motors MUST be initialized here. */
    motorDevInit(&motorConfig()->dev, idlePulse, MAX_SUPPORTED_MOTORS); 
    systemState |= SYSTEM_STATE_MOTORS_READY;

#ifdef USE_PWM
    if (feature(FEATURE_RX_PARALLEL_PWM)) {
        pwmRxInit(pwmConfig());
    }
#endif

#ifdef BEEPER
    beeperInit(beeperDevConfig());
#endif
/* temp until PGs are implemented. */
#if defined(USE_INVERTER) && !defined(SITL)
    initInverters(serialPinConfig());
#endif

#ifdef TARGET_BUS_INIT
    targetBusInit();
#else

#ifdef USE_SPI
    spiPinConfigure();

    // Initialize CS lines and keep them high
    spiPreInit();

#ifdef USE_SPI_DEVICE_1
    spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
    spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
    spiInit(SPIDEV_3);
#endif
#ifdef USE_SPI_DEVICE_4
    spiInit(SPIDEV_4);
#endif
#endif /* USE_SPI */

#ifdef USE_I2C
    i2cHardwareConfigure();

    // Note: Unlike UARTs which are configured when client is present,
    // I2C buses are initialized unconditionally if they are configured.

#ifdef USE_I2C_DEVICE_1
    i2cInit(I2CDEV_1);
#endif
#ifdef USE_I2C_DEVICE_2
    i2cInit(I2CDEV_2);
#endif
#ifdef USE_I2C_DEVICE_3
    i2cInit(I2CDEV_3);
#endif
#ifdef USE_I2C_DEVICE_4
    i2cInit(I2CDEV_4);
#endif
#endif /* USE_I2C */

#endif /* TARGET_BUS_INIT */

#ifdef USE_HARDWARE_REVISION_DETECTION
    updateHardwareRevision();
#endif

#ifdef VTX_RTC6705
    rtc6705IOInit();
#endif

#ifdef USE_CAMERA_CONTROL
    cameraControlInit();
#endif

#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL2)
    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
        serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
    }
#endif

#if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(SONAR) && defined(USE_SOFTSERIAL1)
    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
        serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
    }
#endif

    adcInit();

    initBoardAlignment(boardAlignment());

#ifndef IS_BOAT
    if (!sensorsAutodetect()) {
        // if gyro was not detected due to whatever reason, notify and don't arm.
        indicateFailure(FAILURE_MISSING_ACC, 2);
        setArmingDisabled(ARMING_DISABLED_NO_GYRO);
    }
#endif

    systemState |= SYSTEM_STATE_SENSORS_READY;

    // gyro.targetLooptime set in sensorsAutodetect(),
    // so we are ready to call validateAndFixGyroConfig(), pidInit(), and setAccelerationFilter()
    validateAndFixGyroConfig();
    pidInit(currentPidProfile);
    setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);

#ifdef USE_SERVOS
    servosInit();
    servoConfigureOutput();
    if (isMixerUsingServos()) {
        //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
        servoDevInit(&servoConfig()->dev);
    }
    servosFilterInit();
#endif

    LED1_ON;
    LED0_OFF;
    LED2_OFF;

    for (int i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        if (!(getBeeperOffMask() & (1 << (BEEPER_SYSTEM_INIT - 1)))) BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    imuInit();

    mspFcInit();
    mspSerialInit();

#ifdef USE_CLI
    cliInit(serialConfig());
#endif

    failsafeInit();

    rxInit();

/*
 * CMS, display devices and OSD
 */
#ifdef CMS
    cmsInit();
#endif

#if (defined(OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(CMS)) || defined(USE_OSD_SLAVE))
    displayPort_t *osdDisplayPort = NULL;
#endif

#if defined(OSD) && !defined(USE_OSD_SLAVE)
    //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets

    if (feature(FEATURE_OSD)) {
#if defined(USE_MAX7456)
        // If there is a max7456 chip for the OSD then use it
        osdDisplayPort = max7456DisplayPortInit(vcdProfile());
#elif defined(USE_OSD_OVER_MSP_DISPLAYPORT) // OSD over MSP; not supported (yet)
        osdDisplayPort = displayPortMspInit();
#endif
        // osdInit  will register with CMS by itself.
        osdInit(osdDisplayPort);
    }
#endif

#if defined(USE_OSD_SLAVE) && !defined(OSD)
#if defined(USE_MAX7456)
    // If there is a max7456 chip for the OSD then use it
    osdDisplayPort = max7456DisplayPortInit(vcdProfile());
    // osdInit  will register with CMS by itself.
    osdSlaveInit(osdDisplayPort);
#endif
#endif

#if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
    // If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
    if (!osdDisplayPort)
        cmsDisplayPortRegister(displayPortMspInit());
#endif

#ifdef USE_DASHBOARD
    // Dashbord will register with CMS by itself.
    if (feature(FEATURE_DASHBOARD)) {
        dashboardInit();
    }
#endif


#ifdef GPS
    if (feature(FEATURE_GPS)) {
        gpsInit();
        navigationInit();
    }
#endif

#ifdef LED_STRIP
    ledStripInit();

    if (feature(FEATURE_LED_STRIP)) {
        ledStripEnable();
    }
#endif

#ifdef TELEMETRY
    if (feature(FEATURE_TELEMETRY)) {
        telemetryInit();
    }
#endif

#ifdef USE_ESC_SENSOR
    if (feature(FEATURE_ESC_SENSOR)) {
        escSensorInit();
    }
#endif

#ifdef USB_DETECT_PIN
    usbCableDetectInit();
#endif

#ifdef TRANSPONDER
    if (feature(FEATURE_TRANSPONDER)) {
        transponderInit();
        transponderStartRepeating();
        systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
    }
#endif

#ifdef USE_SDCARD
    if (blackboxConfig()->device == BLACKBOX_DEVICE_SDCARD) {
        sdcardInsertionDetectInit();
        sdcard_init(sdcardConfig()->useDma);
        afatfs_init();
    }
#endif

#ifdef BLACKBOX
    blackboxInit();
#endif

    gyroStartCalibration(false);
#ifdef BARO
    baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif

    ENABLE_STATE(SMALL_ANGLE);

#ifdef SOFTSERIAL_LOOPBACK
    // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
    loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
    if (!loopbackPort->vTable) {
        loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
    }
    serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif

#ifdef USE_DASHBOARD
    if (feature(FEATURE_DASHBOARD)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
        dashboardShowFixedPage(PAGE_GPS);
#else
        dashboardResetPageCycling();
        dashboardEnablePageCycling();
#endif
    }
#endif

#ifdef CJMCU
    LED2_ON;
#endif

#ifdef I_AM_REMOTE_CONTROLLER
    Init_MAX7219();
#endif
    // Latch active features AGAIN since some may be modified by init().
    latchActiveFeatures();
    pwmEnableMotors();

    setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);

#ifdef USE_OSD_SLAVE
    osdSlaveTasksInit();
#else
    fcTasksInit();
#endif

#ifdef USE_RCSPLIT
    rcSplitInit();
#endif // USE_RCSPLIT

    systemState |= SYSTEM_STATE_READY;
}
